#include "levelSet.h"
#include "particleSet.h"
#include "particle.h"


void LevelSet::Update(const Velocity& grid, Double &dt)
{
   //First Order time integration
	FOR_LS
		SemiLagrangianStep(i,j,k,grid,dt);
   END_FOR_THREE
   for(int ifor = 0; ifor<=Nx+1; ifor++)
      for(int jfor = 0; jfor<=Ny+1; jfor++)
      {
         gridTmp(ifor,jfor,0) = gridPhi(ifor,jfor,0);
         gridTmp(ifor,jfor,Nz+1) = gridPhi(ifor,jfor,Nz+1);
      }
   for(int ifor = 0; ifor<=Nx+1; ifor++)
      for(int kfor = 1; kfor<=Nz; kfor++)
      {
         gridTmp(ifor,0, kfor) = gridPhi(ifor,0,kfor);
         gridTmp(ifor,Ny+1,kfor) = gridPhi(ifor,Ny+1,kfor);
      }
   for(int jfor = 1; jfor<=Nx; jfor++)
      for(int kfor = 1; kfor<=Nz; kfor++)
      {
         gridTmp(0,jfor,kfor) = gridPhi(0,jfor,kfor);
         gridTmp(Nx+1,jfor,kfor) = gridPhi(Nx+1,jfor,kfor);
      }
	swap(gridPhi, gridTmp);
	//gridPhi.SetBoundarySignedDist();//Faz Phi=3*HH nas bordas
   //cout<<"\n Voltando da Set_Boundary\n";
}

void LevelSet::SemiLagrangianStep(int x, int y, int z, const Velocity &grid, const Double &dt)
{
   //cout<<"\n Entrando na SemiLagrangean \n";
   static int r,s,t;
   static Double a,b,c;
   int dx[8] ={0,1,0,0,1,1,0,1};
   int dy[8] ={0,0,1,0,1,0,1,1};
   int dz[8] ={0,0,0,1,0,1,1,1};
   double lim_min =0.2*h;
   int control = 0;
   double offset[6][3]={{-1,0,0},{1,0,0},{0,-1,0},{0,1,0},{0,0,-1},{0,0,1}}; 
   double  alpha = 1;
   double uu[3];
	if(gridPhi(x,y,z) > SEMILAGRA_LIMIT) //6.0 * HH; // extent of influence of semi-lagrangian
   {                                   //fora desta faixa a Phi n�o � atualizada
		gridTmp(x,y,z) = gridPhi(x,y,z);
     // if((x>=30) && (z==16) && (y>=15) && (y<=17)) /*gridTmp(x,y,z) < 2.0 * h)*/
     /*{
       cout<<"Out_of_range x,y,z="<<x<<","<<y<<","<<z<<"\n";
       cout<<"gridPhi,gridTemp="<<gridPhi(x,y,z)<<","<<gridTmp(x,y,z)<<"\n";
       //cout<<"a,b,c="<<a<<","<<b<<","<<c<<"\n";
       //cout<<"U="<<u[0]<<","<<u[1]<<","<<u[2]<<"\n";
       //cout<<alpha<<"\n";
     }*/
		return;
	}
	static Vector u; 
   //cout<<"\n Atualizando gridPhi na SemiLagrangean \n";
   //cout<<"x,y,z="<<x<<","<<y<<","<<z<<","<<gridPhi(x,y,z)<<","<<gridTmp(x,y,z)<<"\n";
   u[0] =  grid.gridU(x,y,z) ;
   u[1] =  grid.gridV(x,y,z) ;
   u[2] =  grid.gridW(x,y,z) ;
   //if(x==1 && y==11 && z==1)
    // cout<<"\n u = "<<u[0]<<","<<u[1]<<","<<u[2]<<"\n";
   uu[0] = u[0] * dt * hInv;
   uu[1] = u[1] * dt * hInv; 
   uu[2] = u[2] * dt * hInv;
   //cout<<"\n uu = "<<uu[0]<<","<<uu[1]<<","<<uu[2]<<"\n";
   //uu � o vetor u*dt    
   r = x - int(ceil(uu[0]));//ceil Returns the smallest integral value that is not less than x.
   if(r<1)
   {
      //r = 1;
      alpha = (x-1)/uu[0];
      control = 0;
      if(isnan(alpha) || isinf(alpha))
               {
                  cout<<"***alpha***x**i,j,k="<<x<<","<<y<<","<<z<<"\n";
                  cout<<"***alpha***x**u="<<u[0]<<","<<u[1]<<","<<u[2]<<"\n";
                  exit(0);
               }
   }
   else
      if(r>Nx)
      {
         //r = Nx;
         alpha = (x - Nx)/uu[0];
         control = 1;
          if(isnan(alpha) || isinf(alpha))
         {
            cout<<"***alpha***x**i,j,k="<<x<<","<<y<<","<<z<<"\n";
            cout<<"***alpha***x**u="<<u[0]<<","<<u[1]<<","<<u[2]<<"\n";
            exit(0);
         }

      }
   
	s = y - int(ceil(alpha * uu[1]));//(r,s,t) � a posi��o inteira na malha (anterior) de onde a part�cula veio
   if(s<1)
   {
       alpha = (y-1)/(uu[1]);
       control = 2;
       if(isnan(alpha) || isinf(alpha))
         {
            cout<<"***alpha***y**i,j,k="<<x<<","<<y<<","<<z<<"\n";
            cout<<"***alpha***y**u="<<u[0]<<","<<u[1]<<","<<u[2]<<"\n";
            exit(0);
         }

   }
   else
      if(s>Ny)
      {
         //s = Ny;
         alpha = (y - Ny)/uu[1];
         control = 3;
         if(isnan(alpha) || isinf(alpha))
         {
            cout<<"***alpha**y***i,j,k="<<x<<","<<y<<","<<z<<"\n";
            cout<<"***alpha**y***u="<<u[0]<<","<<u[1]<<","<<u[2]<<"\n";
            exit(0);
         }

      }
   
   t = z - int(ceil(alpha * uu[2]));
   if(t<1)
   {
      //t = 1;
      alpha = (z-1)/uu[2];
      control = 4;
      if(isnan(alpha) || isinf(alpha))
      {
         cout<<"***alpha**z***i,j,k="<<x<<","<<y<<","<<z<<"\n";
         cout<<"***alpha**z***u="<<u[0]<<","<<u[1]<<","<<u[2]<<"\n";
         exit(0);
      }

   }
   else
      if(t>Nz)
      {
         //t = Nz;
         alpha = (z-Nz)/uu[2];
         control = 5;
          if(isnan(alpha) || isinf(alpha))
         {
            cout<<"***alpha*z****i,j,k="<<x<<","<<y<<","<<z<<"\n";
            cout<<"***alpha*z****u="<<u[0]<<","<<u[1]<<","<<u[2]<<"\n";
            exit(0);
         }
      }
   r = x - int(ceil(alpha * uu[0]));
   s = y - int(ceil(alpha * uu[1]));
   t = z - int(ceil(alpha * uu[2]));
   a = Double(x - r)  - alpha *uu[0];//est� entre 0 e 1, diferen�a entre o deslocamento aproximado (inteiro) e o deslocamento 
   b = Double(y - s)  - alpha *uu[1];//real 
   c = Double(z - t)  - alpha *uu[2];
  
   gridTmp(x,y,z) = a*b*c*gridPhi(r+1, s+1, t+1) + (1-a)*b*c* gridPhi(r  , s+1, t+1) +
		        a*(1-b)*c*gridPhi(r+1, s  , t+1) + a*b*(1-c)*gridPhi(r+1, s+1, t  ) +
                     (1-a) * (1-b) *    c  * gridPhi(r  , s  , t+1) +
                     (1-a) *    b  * (1-c) * gridPhi(r  , s+1, t  ) +
                        a  * (1-b) * (1-c) * gridPhi(r+1, s  , t  ) +
		     (1-a) * (1-b) * (1-c) * gridPhi(r  , s  , t  ); 
    if(alpha < 1.0)
   {
      double grid_neib_border_cell = gridPhi(r+offset[control][0], s+offset[control][1], t+offset[control][2]);
      /*cout<<"\n grid_neib="<<grid_neib_border_cell<<"\n";
      cout<<"\n iborder="<<r+offset[control][0]<<"\n";
      cout<<"\n jborder="<<s+offset[control][1]<<"\n";
      cout<<"\n kborder="<<t+offset[control][2]<<"\n";
      cout<<"\n control="<<control<<"\n";*/
      if(grid_neib_border_cell < 0.0)
      {
         double uuxoffset = 0;  
         for(int i=0; i<=2;i++)
             uuxoffset -= uu[i] * offset[control][i];
         uuxoffset = uuxoffset * (1 - alpha);
         //cout<<"\n uuxoffset="<<uuxoffset<<"\n";
         gridTmp(x,y,z) += uuxoffset*( grid_neib_border_cell - gridTmp(x,y,z));
      }
      else if(abs(alpha)>1E-8)
         {
            float beta = (1.0/alpha);
            gridTmp(x,y,z) = gridPhi(x,y,z) + beta * (gridTmp(x,y,z) - gridPhi(x,y,z));
         }
   }
   gridTmp(x,y,z) = fmin(gridTmp(x,y,z),gridPhi(x,y,z));
   if( (gridTmp(x,y,z) > 0) && (gridTmp(x,y,z) < lim_min))
   {
      double grid_local;
      for(int itemp=0;itemp<=7;itemp++)
      {
         grid_local =gridTmp(r+dx[itemp],s+dy[itemp],t+dz[itemp]);
         if(grid_local < 0 && grid_local > -lim_min)
         {
           gridTmp(x,y,z) = gridTmp(r+dx[itemp],s+dy[itemp],t+dz[itemp]);
           break;
         }
      }
   }
   if( (z==16) && (y<=5)) /*gridTmp(x,y,z) < 2.0 * h)*/
  {
    cout<<"x,y,z="<<x<<","<<y<<","<<z<<"\n";
    cout<<"r,s,t,gridPhi,gridTemp="<<r<<","<<s<<","<<t<<","<<gridPhi(x,y,z)<<","<<gridTmp(x,y,z)<<"\n";
    cout<<"Toda gridPhi= "<< gridPhi(r,s,t)<<","<< gridPhi(r+1,s,t)<<","<< gridPhi(r,s+1,t)<<","<< gridPhi(r,s,t+1)
        <<","<< gridPhi(r+1,s+1,t)<<","<< gridPhi(r+1,s,t+1)<<","<< gridPhi(r,s+1,t+1)<<","<< gridPhi(r+1,s+1,t+1)<<"\n";
    cout<<"a,b,c="<<a<<","<<b<<","<<c<<"\n";
    cout<<"U="<<u[0]<<","<<u[1]<<","<<u[2]<<"\n";
    cout<<alpha<<"\n";
  }
   /*if((x>=23)&&(x<=26)&&(y==2)&&(z==16))
         {
            cout<<"x,y="<<x<<","<<y<<","<<gridPhi(x,y,z)<<","<<gridTmp(x,y,z)<<"\n";
         }
   */
//cout<<"\n Saindo da SemiLagrangean \n";
}

void LevelSet::ReInitialize(FastMarch &gridFM, Velocity &gridUVW, Velocity &gridVertexUVW) 
{
    gridFM.Reinitialize(gridPhi, gridUVW, gridVertexUVW, UVWMAX );
    //gridPhi.SetBoundarySignedDist();//Faz Phi=3*HH nas bordas
}
    
void LevelSet::Fix(const ParticleSet& particleSet)
{	
	static ParticleSet::cIterator pit, end;
	static Vector pos;
    static Double phi;
    static int sign;

    end = particleSet.end();
	gridPos = gridPhi;
	gridNeg = gridPhi;
	for(pit = particleSet.begin(); pit != end; ++pit)
	{
		Particle& particle = *(*pit);
		particle.GetPosition(pos);
        phi = SAMPLEPHI(pos);
		sign = particle.Sign();

		if(phi * sign < 0.)
		{
			//particle has crossed the boundary
			if(sign < 0.) FixNeg(particle, int(pos[0]), int(pos[1]), int(pos[2]));
			else		  FixPos(particle, int(pos[0]), int(pos[1]), int(pos[2]));
		}
	}
	//Merge gridPos & gridNeg
	static Double phiPos, phiNeg;
	FOR_ALL_LS
		phiPos = gridPos(i,j,k); phiNeg = gridNeg(i,j,k);
		gridPhi(i,j,k) = abs(phiPos) < abs(phiNeg) ? phiPos : phiNeg;
	END_FOR_THREE
}

inline void LevelSet::FixNeg(const Particle &particle, int i, int j, int k)
{
	static Double particlePhi;
	for(int dx = 0; dx < 2; dx++) {
		for(int dy = 0; dy < 2; dy++) {
            for(int dz = 0; dz < 2; dz++) {
			    particlePhi = particle.phi(Vector(i+dx,j+dy,k+dz), h);
                gridNeg(i+dx,j+dy,k+dz) = min(particlePhi, gridNeg(i+dx,j+dy,k+dz));
            }
		}
	}
}

inline void LevelSet::FixPos(const Particle &particle, int i, int j, int k)
{
	static Double particlePhi;
	for(int dx = 0; dx < 2; dx++) {
		for(int dy = 0; dy < 2; dy++) {
            for(int dz = 0; dz < 2; dz++) {
			    particlePhi = particle.phi(Vector(i+dx,j+dy,k+dz), h);
                gridPos(i+dx,j+dy,k+dz) = max(particlePhi, gridPos(i+dx,j+dy,k+dz));
            }
		}
    }
}

//inline Double LevelSet::LinearSample(const Vector &pos) const
//foi definida no .h porque � inline e � chamada fora desse cpp, portanto tem que ser declarada l�,
//ou seja, fun��es inline devem ter seu c�digo incluido no local onde s�o chamadas, por isso est� no .h

Double LevelSet::CubicSample(const Vector &pos) const
{
    static Double r, s, t;
    static int i0,i1,i2,i3,j0,j1,j2,j3,k0,k1,k2,k3;
    
    i0 = int(pos[0]) - 1; i1 = i0+1; i2 = i1+1; i3 = i2+1;
    j0 = int(pos[1]) - 1; j1 = j0+1; j2 = j1+1; j3 = j2+1;
    k0 = int(pos[2]) - 1; k1 = k0+1; k2 = k1+1; k3 = k2+1;
    r = pos[0] - i1;
    s = pos[1] - j1;
    t = pos[2] - k1;
    i0 = i0 < 0 ? 0 : i0; i3 = i3 > Nx+1 ? Nx+1 : i3;
    j0 = j0 < 0 ? 0 : j0; j3 = j3 > Ny+1 ? Ny+1 : j3;
    k0 = k0 < 0 ? 0 : k0; k3 = k3 > Nz+1 ? Nz+1 : k3;

	return  MCerp(t, MCerp(r, MCerp(s, gridPhi(i0,j0,k0),gridPhi(i0,j1,k0),
                              gridPhi(i0,j2,k0),gridPhi(i0,j3,k0)),
                              MCerp(s, gridPhi(i1,j0,k0),gridPhi(i1,j1,k0),
                              gridPhi(i1,j2,k0),gridPhi(i1,j3,k0)),
                              MCerp(s, gridPhi(i2,j0,k0),gridPhi(i2,j1,k0),
                              gridPhi(i2,j2,k0),gridPhi(i2,j3,k0)),
                              MCerp(s, gridPhi(i3,j0,k0),gridPhi(i3,j1,k0),
                              gridPhi(i3,j2,k0),gridPhi(i3,j3,k0)) ),
                     MCerp(r, MCerp(s, gridPhi(i0,j0,k1),gridPhi(i0,j1,k1),
                              gridPhi(i0,j2,k1),gridPhi(i0,j3,k1)),
                              MCerp(s, gridPhi(i1,j0,k1),gridPhi(i1,j1,k1),
                              gridPhi(i1,j2,k1),gridPhi(i1,j3,k1)),
                              MCerp(s, gridPhi(i2,j0,k1),gridPhi(i2,j1,k1),
                              gridPhi(i2,j2,k1),gridPhi(i2,j3,k1)),
                              MCerp(s, gridPhi(i3,j0,k1),gridPhi(i3,j1,k1),
                              gridPhi(i3,j2,k1),gridPhi(i3,j3,k1)) ),
                     MCerp(r, MCerp(s, gridPhi(i0,j0,k2),gridPhi(i0,j1,k2),
                              gridPhi(i0,j2,k2),gridPhi(i0,j3,k2)),
                              MCerp(s, gridPhi(i1,j0,k2),gridPhi(i1,j1,k2),
                              gridPhi(i1,j2,k2),gridPhi(i1,j3,k2)),
                              MCerp(s, gridPhi(i2,j0,k2),gridPhi(i2,j1,k2),
                              gridPhi(i2,j2,k2),gridPhi(i2,j3,k2)),
                              MCerp(s, gridPhi(i3,j0,k2),gridPhi(i3,j1,k2),
                              gridPhi(i3,j2,k2),gridPhi(i3,j3,k2)) ),
                     MCerp(r, MCerp(s, gridPhi(i0,j0,k3),gridPhi(i0,j1,k3),
                              gridPhi(i0,j2,k3),gridPhi(i0,j3,k3)),
                              MCerp(s, gridPhi(i1,j0,k3),gridPhi(i1,j1,k3),
                              gridPhi(i1,j2,k3),gridPhi(i1,j3,k3)),
                              MCerp(s, gridPhi(i2,j0,k3),gridPhi(i2,j1,k3),
                              gridPhi(i2,j2,k3),gridPhi(i2,j3,k3)),
                              MCerp(s, gridPhi(i3,j0,k3),gridPhi(i3,j1,k3),
                              gridPhi(i3,j2,k3),gridPhi(i3,j3,k3)) ) );
}

void LevelSet::normal(const Vector &pos, Vector &n) const
{
	gradient(pos, n);
	n.Normalize();
}

void LevelSet::gradient(const Vector &pos, Vector &g) const
{
	static Vector dx (0.25, 0.0, 0.0);
	static Vector dy (0.0, 0.25, 0.0);
   static Vector dz (0.0, 0.0, 0.25);
	g[0] = SAMPLEPHI(pos+dx) - SAMPLEPHI(pos-dx);
	g[1] = SAMPLEPHI(pos+dy) - SAMPLEPHI(pos-dy);
    g[2] = SAMPLEPHI(pos+dz) - SAMPLEPHI(pos-dz);
	g *= 2. * hInv;
}

void LevelSet::gradient(const Vector &pos, const Vector &u, Vector &g)
{
	static Vector dx (0.25 , 0.  , 0.  );
	static Vector dy (0.   , 0.25, 0.  );
   static Vector dz (0.   , 0.  , 0.25);
   static Double center;
	center = SAMPLEPHI(pos);
	if(u[0] < 0)	g[0] = (SAMPLEPHI(pos + dx) - center);
	else			g[0] = (center - SAMPLEPHI(pos - dx));
	if(u[1] < 0)	g[1] = (SAMPLEPHI(pos + dy) - center);
	else			g[1] = (center - SAMPLEPHI(pos - dy));
    if(u[2] < 0)	g[2] = (SAMPLEPHI(pos + dz) - center);
	else			g[2] = (center - SAMPLEPHI(pos - dz));
    g *= 4 * hInv;
}

